#include "uni_motor.h"

/*--------------------------------------------------------------------------------------------------------------
功能：构造函数定义1. 用来声明编码电机对象.
概述：该构造函数为使用本实验提供的编码电机，方便开发者使用
函数原型：UNI_MOTOR(int motor_label);
函数讲解：
    【显示的形式参数】：
      int motor_label： 表示电机标签
            如果motor_label=1:表示使用的编码电机引脚为：电机方向1:37  电机方向2:36  电机pwm:8   编码器A相:18  编码器B相:32
            如果motor_label=2:表示使用的编码电机引脚为：电机方向1:A2  电机方向2:A3  电机pwm:5   编码器A相:19  编码器B相:38
            如果motor_label=3:表示使用的编码电机引脚为：电机方向1:34  电机方向2:35  电机pwm:12  编码器A相:20  编码器B相:33
            如果motor_label=4:表示使用的编码电机引脚为：电机方向1:43  电机方向2:42  电机pwm:9   编码器A相:21  编码器B相:39
            如果motor_label=5:表示使用的编码电机引脚为：电机方向1:31  电机方向2:30  电机pwm:6   编码器A相:3   编码器B相:41
            如果motor_label=6:表示使用的编码电机引脚为：电机方向1:49  电机方向2:48  电机pwm:4   编码器A相:2   编码器B相:40
    【省略的参数】：
      float _motor_kp: 100.0
      float _motor_ki: 0.0
      float _motor_kd: 500.0
      float _motor_pulse: 1560 (本实验用的编码电机转动一圈，编码器计数为1560)
      float _motor_radius: 0.04米 (本实验用的编码电机采用默认的外轮直径为8厘米，故这里的半径为0.04米)  */
UNI_MOTOR::UNI_MOTOR(int motor_label){
  uni_common_mode(motor_label);
  _motor_kp = 100.0;
  _motor_ki = 0.0;
  _motor_kd = 500.0;
  _motor_pulse = 1560;
  _motor_radius = 0.04;
  uni_motor_begin();
}


/*--------------------------------------------------------------------------------------------------------------------------
功能：构造函数定义4. 用来声明编码电机对象.
概述：该构造函数可以使用L298N驱动的编码电机，可以使用自己的，或者是其他厂家提供的编码电机，方便开发者使用
函数原型：UNI_MOTOR(int m_1, int m_2, int pwm, int e_a, int e_b);
函数讲解：
  int m_1: 表示控制编码电机方向的引脚1
  int m_2: 表示控制编码电机方向的引脚2
  int pwm: 表示控制编码电机PWM脉宽调制的引脚
  int e_a: 表示编码电机编码器A相引脚
  int e_b: 表示编码电机编码器B相引脚
  【省略的参数】：
  float kp: 表示设定pid调控中的kp的值
  float ki: 表示设定pid调控中的ki的值
  float kd: 表示设定pid调控中的kd的值
  float pulse: 表示车轮转动一圈编码器触发的总次数.计算方式如下：
              pulse = ppr(电机线圈数) * 2 * reduction(电机减速比) * 2
              本实验中使用的编码电机脉冲数 pulse = 13 * 2 * 30 * 2 = 1560
  float _motor_radius: 0.04米 (本实验用的编码电机采用默认的外轮直径为8厘米，故这里的半径为0.04米)  */
UNI_MOTOR::UNI_MOTOR(int m_1, int m_2, int pwm, int e_a, int e_b):
  _motor_pin1(m_1), _motor_pin2(m_2), _motor_pwm(pwm), _motor_enca(e_a), _motor_encb(e_b){
  _motor_kp = 100.0;
  _motor_ki = 0.0;
  _motor_kd = 500.0;
  _motor_pulse = 1560;
  _motor_radius = 0.04;
}


/*--------------------------------------------------------------------------------------------------------------------------
功能：构造函数定义4. 用来声明编码电机对象.
概述：该构造函数可以使用L298N驱动的编码电机，可以使用自己的，或者是其他厂家提供的编码电机，方便开发者使用
函数原型：UNI_MOTOR(int m_1, int m_2, int pwm, int e_a, int e_b, float kp, float ki, float kd, float pulse, float radius);
函数讲解：
  int m_1: 表示控制编码电机方向的引脚1
  int m_2: 表示控制编码电机方向的引脚2
  int pwm: 表示控制编码电机PWM脉宽调制的引脚
  int e_a: 表示编码电机编码器A相引脚
  int e_b: 表示编码电机编码器B相引脚
  float kp: 表示设定pid调控中的kp的值
  float ki: 表示设定pid调控中的ki的值
  float kd: 表示设定pid调控中的kd的值
  float pulse: 表示车轮转动一圈编码器触发的总次数.计算方式如下：
              pulse = ppr(电机线圈数) * 2 * reduction(电机减速比) * 2
              本实验中使用的编码电机脉冲数 pulse = 13 * 2 * 30 * 2 = 1560
  float _motor_radius: 0.04米 (本实验用的编码电机采用默认的外轮直径为8厘米，故这里的半径为0.04米) ------------------------------*/
UNI_MOTOR::UNI_MOTOR(int m_1, int m_2, int pwm, int e_a, int e_b, float kp, float ki, float kd, float pulse, float radius):
_motor_pin1(m_1), _motor_pin2(m_2), _motor_pwm(pwm), _motor_enca(e_a), _motor_encb(e_b), _motor_kp(kp), 
_motor_ki(ki), _motor_kd(kd), _motor_pulse(pulse), _motor_radius(radius) {
  uni_motor_begin();
}


/*************************************************************************
电机参数初始化设置,包含设置编码电机pid算法中需要用到的相关参数.
enc=new Encoder(_motor_enca, _motor_encb):构建编码电机编码器A，B相编码器容器
_motor_ki_error_sum：初始化pid中ki积分变量
_motor_deltaEnc：初始化编码器变化量
_motor_bias：初始化当前误差
_motor_last_bias：初始化上一次误差
_motor_output_pwm：初始化电机输出pwm脉宽
_motor_current_wheel_vel：初始化电机当前速度
_motor_target_wheel_vel：初始化电机要到达的目标速度
_motor_resolution：计算编码电机分辨率
**************************************************************************/
void UNI_MOTOR::uni_motor_begin(){
  enc=new Encoder(_motor_enca, _motor_encb);
  _motor_ki_error_sum = 0;
  _motor_deltaEnc = 0;
  _motor_deltaT = 0.1;
  _motor_bias = 0;
  _motor_last_bias = 0;
  _motor_output_pwm = 0.0;
  _motor_current_wheel_vel = 0.0;
  _motor_target_wheel_vel = 0.0;
  _motor_resolution = 2 * PI * _motor_radius / _motor_pulse;
  _motor_reverse = true;
}


/******************************************************************************************************
功能:选择要声明的电机标签对应的引脚号
函数原型：void uni_common_mode(int choose);
函数讲解：
  choose: 表示要选择声明的电机标签号所对应的电机引脚
      choose=1:表示使用的编码电机引脚为：电机方向1:37  电机方向2:36  电机pwm:8   编码器A相:18  编码器B相:32
      choose=2:表示使用的编码电机引脚为：电机方向1:A2  电机方向2:A2  电机pwm:5   编码器A相:19  编码器B相:38
      choose=3:表示使用的编码电机引脚为：电机方向1:34  电机方向2:35  电机pwm:12  编码器A相:20  编码器B相:33
      choose=4:表示使用的编码电机引脚为：电机方向1:43  电机方向2:42  电机pwm:9   编码器A相:21  编码器B相:39
      choose=5:表示使用的编码电机引脚为：电机方向1:31  电机方向2:30  电机pwm:6   编码器A相:3   编码器B相:41
      choose=6:表示使用的编码电机引脚为：电机方向1:49  电机方向2:48  电机pwm:4   编码器A相:2   编码器B相:40
*******************************************************************************************************/
void UNI_MOTOR::uni_common_mode(int choose){
  switch(choose){
    case UNI_MOTOR_1:{_motor_pin1 = 37; _motor_pin2 = 36; _motor_pwm  = 8;  _motor_enca = 18; _motor_encb = 32;}break;
    case UNI_MOTOR_2:{_motor_pin1 = A3; _motor_pin2 = A2; _motor_pwm  = 5;  _motor_enca = 19; _motor_encb = 38;}break;
    case UNI_MOTOR_3:{_motor_pin1 = 34; _motor_pin2 = 35; _motor_pwm  = 12; _motor_enca = 20; _motor_encb = 33;}break;
    case UNI_MOTOR_4:{_motor_pin1 = 43; _motor_pin2 = 42; _motor_pwm  = 9;  _motor_enca = 21; _motor_encb = 39;}break;
    case UNI_MOTOR_5:{_motor_pin1 = 31; _motor_pin2 = 30; _motor_pwm  = 6;  _motor_enca = 3;  _motor_encb = 41;}break;
    case UNI_MOTOR_6:{_motor_pin1 = 49; _motor_pin2 = 48; _motor_pwm  = 4;  _motor_enca = 2;  _motor_encb = 40;}break;
    default: break;
  }
}

/*设置编码电机电机方向、调速引脚为输出模式*/
void  UNI_MOTOR::motor_pin_init(){
  pinMode(_motor_pin1, OUTPUT);
  pinMode(_motor_pin2, OUTPUT);
  pinMode(_motor_pwm, OUTPUT);
  pinMode(_motor_enca, INPUT);
  pinMode(_motor_encb, INPUT);
}

/*************************************************
 * 获取电机库版本号
 * return MOTOR_LIB_VERSION(对应编码电机库版本号)
**************************************************/
float  UNI_MOTOR::get_version(){

}


/*************************************************************************
设置编码电机线圈数，减速比，轮子半径
本实验使用的
  编码电机线圈数ppr为：13  
  电机减速比为：30  
  连接编码电机的轮子半径为0.04米
如果使用的是其他型号的编码电机，修改为对应电机参数即可。
*************************************************************************/
void  UNI_MOTOR::set_parameters(float ppr, float reduction, float radius){
  _motor_radius = radius;
  _motor_pulse = ppr * 2 * reduction * 2;
  _motor_resolution = 2 * PI * radius / _motor_pulse;
}


/* 析构函数定义. 用来程序结束后,删除声明的电机.该函数目前无实际作用，后期可能会考虑重构该函数. */
UNI_MOTOR::~UNI_MOTOR(){
  
}


//电机引脚初始化，以及准备开始新的控制周期
void UNI_MOTOR::init(){
  _motor_lastTime=0;
  _motor_lastEnc=0;
  motor_pin_init();
}


/*****************************************************************
功能：更新编码器时间、编码器值
函数原型：tic();
_motor_currentTime：更新编码器当前时间
_motor_currentEnc：读取编码器当前值，并保存到_motor_currentEnc变量中
返回：无
*****************************************************************/
void UNI_MOTOR::tic(){
  _motor_currentTime = millis();
  _motor_currentEnc  = enc->read();
  _motor_currentEnc  = _motor_reverse ? -_motor_currentEnc : _motor_currentEnc; //如果 _motor_reverse 为真，则编码器读数取反。
  _save_all_encoder_data = _motor_currentEnc;
  //Serial.print("currentEnc:");Serial.println(_motor_currentEnc);
}


/**********************************************************
功能：编码电机一个周期，编码器数值更新函数
函数原型：toc();
_motor_lastTime：保存编码器上一次的周期时间
_motor_lastEnc：保存编码器上一次的编码器值
返回：无
*********************************************************/
void UNI_MOTOR::toc(){
  _motor_lastTime = _motor_currentTime;
  _motor_lastEnc = _motor_currentEnc;
}


/*****************************************************
功能：获取编码电机一个周期所用的时间
函数原型：void getDeltaT();
函数讲解：获取编码电机的编码器的一个周期时间
返回： float _motor_deltaT;
******************************************************/
float UNI_MOTOR::getDeltaT(){
  return _motor_deltaT;
}


/*****************************************************
功能：获取编码电机编码器值
函数原型：void get_encoder();
函数讲解：获取编码电机编码器值（从开机到结束时的编码器总值）
返回： long _motor_currentEnc;
******************************************************/
long UNI_MOTOR::get_encoder(){
  return _motor_currentEnc;
}


/*****************************************************
功能：获取编码电机当前位置函数
函数原型：float get_pos();
函数讲解：实时记录编码电机当前位置
返回： float _save_encoder_current_pos;返回编码电机位置
******************************************************/
float UNI_MOTOR::get_pos(){
  tic();
  _save_encoder_current_pos = _save_all_encoder_data * _motor_resolution;
  return _save_encoder_current_pos;
}

/*****************************************************
功能：获取电机实时速度
函数原型：void get_current_vel();
函数讲解：获取电机实时速度
返回： float _motor_current_wheel_vel;
******************************************************/
float UNI_MOTOR::get_current_vel(){
  return _motor_current_wheel_vel;
}


/*****************************************************
功能：获取电机需要到达的目标速度
函数原型：void get_target_vel();
函数讲解：
    当我们调用set_vel(float target_vel)函数后，程序会
    存储电机要到达的目标速度，该函数可获取电机的目标速度.
返回： float _motor_target_wheel_vel;
******************************************************/
float UNI_MOTOR::get_target_vel(){
  return _motor_target_wheel_vel;
}


/*****************************************************
功能：设置电机速度.速度模式
函数原型：void set_vel(float target_vel);
函数讲解：
    target_vel: 设定电机需要到达的速度值 m/s
    速度范围：建议[-1.3 ~ 1.3] m/s
    -1.3表示电机反转最大速度值，1.3表示电机正转最大速度值.
返回： float _motor_target_wheel_vel;
******************************************************/
float UNI_MOTOR::set_vel(float target_vel){
  _motor_working_mode = UNI_WORK_VEL; //记录电机当前为速度工作模式
  if(target_vel >= UNI_MOTOR_VEL_MAX){
    target_vel = UNI_MOTOR_VEL_MAX;
  }
  if(target_vel <= UNI_MOTOR_VEL_MIN){
    target_vel = UNI_MOTOR_VEL_MIN;
  }
  _motor_target_wheel_vel = target_vel;
}


/*******************************************************************
功能：设置电机速度.pwm模式
函数原型：void set_pwm(int pwm);
函数详解：
  pwm:范围[-255 ~ 255] -255~0:表示电机正转，0~255表示电机反转，0表示停止
返回：无
********************************************************************/
void UNI_MOTOR::set_pwm(int pwm){
  _motor_working_mode = UNI_WORK_PWM; //记录电机当前为PWM工作模式
  if(pwm >= 1){
    do{digitalWrite(_motor_pin1, LOW);  digitalWrite(_motor_pin2, HIGH); analogWrite(_motor_pwm, pwm);}while(0);
  }
  else if(pwm <= -1){
    do{digitalWrite(_motor_pin1, HIGH); digitalWrite(_motor_pin2, LOW);  analogWrite(_motor_pwm, abs(pwm));}while(0);
  }
  else{
    do{digitalWrite(_motor_pin1, HIGH); digitalWrite(_motor_pin2, HIGH); analogWrite(_motor_pwm, 0);}while(0);
  }
}


/****************************************************
功能：修改编码电机速度环pid参数
函数原型:void set_vel_pid(float kp, float ki, float kd);
函数详解：
  kp:修改编码电机速度环kp的值
  ki:修改编码电机速度环ki的值
  kd:修改编码电机速度环kd的值
返回：无
*****************************************************/
void UNI_MOTOR::set_vel_pid(float kp, float ki, float kd){
  _motor_kp = kp;
  _motor_ki = ki;
  _motor_kd = kd;
}

/****************************************************
功能：修改编码电机位置环pid参数
函数原型:void set_pos_pid(float kp, float ki, float kd);
函数详解：
  kp:修改编码电机位置环kp的值
  ki:修改编码电机位置环ki的值
  kd:修改编码电机位置环kd的值
返回：无
*****************************************************/
void UNI_MOTOR::set_pos_pid(float kp, float ki, float kd){
  _out_kp = kp;
  _out_ki = ki;
  _out_kd = kd;
}


/*****************************************************************
功能：电机位置模式下，更新编码器位置
函数原型：tic();
_motor_currentEnc：读取编码器当前值，并保存到_motor_currentEnc变量中
返回：无
*****************************************************************/
void UNI_MOTOR::update_pos_encoder(){
  _motor_currentEnc  = enc->read();
  _motor_currentEnc  = _motor_reverse ? -_motor_currentEnc : _motor_currentEnc; //如果_motor_reverse为真,则编码器读数取反。
  _save_all_encoder_data = _motor_currentEnc;
}


/********************************************************************
功能：实现通用编码电机速度pid调控函数
函数原型: void speed_loop();
函数详解：该函数为编码电机pid速度调控的原理函数.
  供update();函数中pid速度调控的底层代码使用.
返回：无
********************************************************************/
void  UNI_MOTOR::speed_loop(){
  tic();
  _motor_deltaEnc = _motor_currentEnc - _motor_lastEnc;
  _motor_deltaT = (_motor_currentTime - _motor_lastTime) / 1000.0;
  _motor_current_wheel_vel = _motor_deltaEnc * _motor_resolution / _motor_deltaT;
  _motor_bias = _motor_current_wheel_vel - (_motor_target_wheel_vel);

  _motor_ki_error_sum += _motor_ki * _motor_bias;
  float kd_value = _motor_bias - _motor_last_bias;
  _motor_output_pwm += _motor_kp * _motor_bias + _motor_ki_error_sum + _motor_kd * kd_value;

  _motor_last_bias = _motor_bias;
  _motor_output_pwm = constrain(_motor_output_pwm, _motor_pwm_min, _motor_pwm_max);
  set_pwm(_motor_output_pwm);
  toc();
}


/********************************************************************
功能：设定电机需要到达的位置
函数原型:void set_pos(float target_pos);
函数讲解：该函数为设定电机需要到达的位置
********************************************************************/
void UNI_MOTOR::set_pos(float target_pos){
  // _motor_working_mode = UNI_WORK_POS; //记录电机当前为位置工作模式
  // pos_flag = 1;
  // _motor_target_position = target_pos / _motor_resolution; //记录电机需要到达的位置
  // update_pos_encoder();
  // _motor_target_position = _motor_currentEnc + (target_pos / _motor_resolution);
  // _motor_target_encoder_pos = _motor_target_position * _motor_resolution;
  // pos_loop();

  _motor_working_mode = UNI_WORK_POS; //记录电机当前为位置工作模式
  pos_flag = 1;
  _motor_target_position = target_pos / _motor_resolution; //记录电机需要到达的位置
  update_pos_encoder();
  _motor_target_position = (target_pos / _motor_resolution);
  _motor_target_encoder_pos = _motor_target_position * _motor_resolution;

  pos_loop();
}


/********************************************************************
功能：封装编码电机位置环-速度环双环pid控制函数
函数原型: void pos_loop();
函数详解：
  原理：该函数为双环(位置环-速度环)pid控制函数.外环为位置环，内环为速度环
  外环检测编码电机是否到达指定目标位置，内环为检测编码电机是否按照外环输出值
  运动.
  工作流程:当指定编码电机目标位置后，外环位置pid将输出得到的值传给内环速度环
  的目标速度. 直到外环检测到已到达目标位置，退出双环pid控制函数.
返回：无
********************************************************************/
void UNI_MOTOR::pos_loop(){
  //外环位置环
  _current_encoder_pos = _motor_currentEnc * _motor_resolution;
  _out_pos_error = _motor_target_encoder_pos - _current_encoder_pos;
  _out_integral_pos += (_out_pos_error);
  _out_derivative_pos = (_out_pos_error - _out_pos_lasterror);
  _out_output_vel = _out_kp * _out_pos_error + _out_ki * _out_integral_pos + _out_kd * _out_derivative_pos;
  _out_pos_lasterror = _out_pos_error;

  _out_output_vel = constrain(_out_output_vel, _motor_vel_pid_min, _motor_vel_pid_max);
  if(fabs(_motor_target_encoder_pos - _current_encoder_pos) < _out_pos_delta_error){
    pos_vel_count ++;
    _out_output_vel = 0;
    if(pos_vel_count >= 10){
      pos_parameters_reset(); //位置模式下，复位位置模式参数
    }
  }
  //内环速度环
  _motor_target_wheel_vel = _out_output_vel;
  speed_loop();
  delayMicroseconds(1000); //编码电机死驱时间
}

//复位双环pid模式参数值
void UNI_MOTOR::pos_parameters_reset(){
  _motor_working_mode = 0; //记录电机退出双环pid模式
  pos_vel_count = 0;
  pos_flag = 0;
  //清空内环速度环相关参数
  _motor_bias = 0;
  _motor_ki_error_sum = 0;
  _motor_output_pwm = 0;
  _motor_last_bias = 0;
  
  //清空外环位置环相关参数
  _out_pos_error = 0;
  _out_integral_pos = 0;
  _out_derivative_pos = 0;
  _out_output_vel = 0;
  _out_pos_lasterror = 0;
}

/********************************************************************
功能：刷新电机状态参数函数
函数原型:void update();
函数详解：该函数内部设定了_motor_working_mode参数用以判断编码电机工作模式
  如果_motor_working_mode = UNI_WORK_VEL;即编码电机处于速度模式下时：
      编码电机执行speed_loop(); 速度pid调控方式.
  如果_motor_working_mode = UNI_WORK_POS;即编码电机处于位置模式下时：
      本程序通过位置环-速度环双环pid控制模式实现编码电机的位置pid调试.
返回：无
********************************************************************/
void UNI_MOTOR::update(){
  switch(_motor_working_mode){
    //如果编码电机当前位于速度模式(单环速度pid控制)
    case UNI_WORK_VEL:{  speed_loop();  }break;

    //如果编码电机当前位于位置模式(双环pid控制)
    case UNI_WORK_POS:{  pos_loop();  }break;

    //是编码电机保持在当前位置(双环pid控制)
    default: {  keep_current_pos();  }break;
  }
}

//编码电机运行到指定位置后，使用该函数是编码电机保持在当前位置
void UNI_MOTOR::keep_current_pos(){
  //外环位置环控制
  _current_encoder_pos = _motor_currentEnc * _motor_resolution;
  _out_pos_error = _motor_target_encoder_pos - _current_encoder_pos;
  _out_integral_pos += (_out_pos_error);
  _out_derivative_pos = (_out_pos_error - _out_pos_lasterror);
  _out_output_vel = _out_kp * _out_pos_error + _out_ki * _out_integral_pos + _out_kd * _out_derivative_pos;
  _out_pos_lasterror = _out_pos_error;
  //限制编码电机到达目标位置时，能到达的最大/最小速度（单位：m/s）
  _out_output_vel = constrain(_out_output_vel, _motor_vel_pid_min, _motor_vel_pid_max);
  if(fabs(_motor_target_encoder_pos - _current_encoder_pos) < _out_pos_delta_error){
    _out_output_vel = 0;
  }
  //内环速度环控制
  _motor_target_wheel_vel = _out_output_vel;
  speed_loop();
  delayMicroseconds(1000);
}


/****************************************************
功能：打印电机当前参数值
函数原型:void get_parameters();
函数详解：
    _motor_pin1： 编码电机方向引脚1
    _motor_pin2： 编码电机方向引脚2
    _motor_pwm：  编码电机PWM引脚
    _motor_enca： 编码电机编码器引脚A相
    _motor_encb： 编码电机编码器引脚B相
    _motor_kp：   当前电机Kp的值
    _motor_ki：   当前电机Ki的值
    _motor_kd：   当前电机Kd的值
    _motor_pulse： 编码电机脉冲数
    _motor_radius： 车轮半径
返回：无
*****************************************************/
/*  打印当前电机参数信息,包含了当前编码电机的引脚，编码传感器引脚，pid参数, 脉冲数，车轮半径  */
void UNI_MOTOR::get_parameters(){
  Serial.print("编码电机方向1引脚: ");Serial.println(_motor_pin1);
  Serial.print("编码电机方向2引脚: ");Serial.println(_motor_pin2);
  Serial.print("编码电机PWM引脚: ");Serial.println(_motor_pwm);
  Serial.print("编码电机编码器引脚A相: ");Serial.println(_motor_enca);
  Serial.print("编码电机编码器引脚B相: ");Serial.println(_motor_encb);
  Serial.print("PID参数中的Kp: ");Serial.println(_motor_kp, 4);
  Serial.print("PID参数中的Ki: ");Serial.println(_motor_ki, 4);
  Serial.print("PID参数中的KD: ");Serial.println(_motor_kd, 4);
  Serial.print("编码电机脉冲数: ");Serial.println(_motor_pulse, 4);
  Serial.print("车轮半径: ");Serial.print(_motor_radius, 6);Serial.println("米");
  Serial.print("编码电机分辨率: ");Serial.println(_motor_resolution, 6);
}

